/*
 * 不使用定时器的舵机驱动实现
 * 通过delayMicroseconds生成PWM信号
 */

// 定义舵机控制引脚
#define SERVO_PIN 23

// 舵机参数
#define MIN_PULSE_WIDTH 500   // 最小脉冲宽度(微秒)，对应0°
#define MAX_PULSE_WIDTH 2500  // 最大脉冲宽度(微秒)，对应180°
#define PERIOD 20             // PWM周期(毫秒)，20ms = 50Hz

int currentAngle = 90;  // 当前角度

void setup() {
  Serial.begin(115200);
  // 初始化舵机控制引脚为输出
  pinMode(SERVO_PIN, OUTPUT);

  // 初始化舵机到90度位置
  setServoAngle(90);
  Serial2.begin(115200);
}

void loop() {

  delay(100);

  // 演示：从0°转到180°，再转回来
  for (int angle = 0; angle <= 180; angle++) {
    setBusServoAngle(angle);
    Serial.println(angle);
    delay(15);  // 等待舵机移动到目标位置
  }

  for (int angle = 180; angle >= 0; angle--) {
    setBusServoAngle(angle);
    Serial.println(angle);
    delay(15);  // 等待舵机移动到目标位置
  }
}

void setBusServoAngle(int angle) {
  if (angle < 0) angle = 0;
  if (angle > 180) angle = 180;

  currentAngle = angle;

  // 计算脉冲宽度(微秒)
  long pulseWidth = map(angle, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
  String duty = String(pulseWidth);

  for (int i = 0; i < 4 - duty.length(); i++)
    duty = "0" + duty;

  Serial.println(duty);

  Serial2.print("#000P"+duty+"T0100!");
}

/*
 * 设置舵机角度
 * angle: 目标角度(0-180)
 */
void setServoAngle(int angle) {
  // 限制角度范围
  if (angle < 0) angle = 0;
  if (angle > 180) angle = 180;

  currentAngle = angle;

  // 计算脉冲宽度(微秒)
  long pulseWidth = map(angle, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);

  // 生成一个PWM周期信号
  digitalWrite(SERVO_PIN, HIGH);  // 输出高电平
  delayMicroseconds(pulseWidth);  // 保持高电平pulseWidth微秒
  digitalWrite(SERVO_PIN, LOW);   // 输出低电平

  // 保持低电平直到完成20ms周期
  // 减去已经延迟的时间，确保总周期为20ms
  delay(PERIOD - (pulseWidth / 1000));
}
